//
// Created by chen on 25-5-9.
//

#include "INA199A1DCKR.h"
#include "../FOC/FOC.h"
#include "../SVPWM/SVPWM.h"
#include "../startupdir/globledata.h"
#include "../FOC/motor_runtime_param.h"
extern ADC_HandleTypeDef hadc1;
extern volatile float current_u;
extern volatile float current_v;
extern volatile float angle;
extern  TIM_HandleTypeDef htim1;
// 静态实例
INA199A1DCKR INA199A1DCKR::s_ina199;

// 获取单例实例
INA199A1DCKR& INA199A1DCKR::instance() {
    return s_ina199;
}

// 构造函数
INA199A1DCKR::INA199A1DCKR()
    : m_shuntResistance(0.1),
      m_inaGain(20),
      m_currentPhaseIndex(0),
      m_conversionComplete(false) {
    // 默认配置：PHASE_U -> ADC_CHANNEL_0, PHASE_V -> ADC_CHANNEL_1
    m_adcChannels[PHASE_U] = ADC_CHANNEL_0;
    m_adcChannels[PHASE_V] = ADC_CHANNEL_1;
}

// 初始化函数
void INA199A1DCKR::init(float shuntResistance, float inaGain) {
    m_shuntResistance = shuntResistance;
    m_inaGain = inaGain;
}

// 配置相序与ADC通道映射
void INA199A1DCKR::configurePhaseChannel(Phase phase, uint32_t adcChannel) {
    if (phase < PHASE_COUNT) {
        m_adcChannels[phase] = adcChannel;
    }
}

// ADC回调函数
void INA199A1DCKR::adcCallback() {
    // 读取当前相的ADC值
    m_adcValues[m_currentPhaseIndex] = HAL_ADC_GetValue(&hadc1);

    // 转换为电流值
    m_currentValues[m_currentPhaseIndex] = convertToCurrent(m_adcValues[m_currentPhaseIndex]);
    if (m_currentPhaseIndex==0) {
        current_u=m_currentValues[m_currentPhaseIndex];
    }else {
        current_v=m_currentValues[m_currentPhaseIndex];
    }

    // 切换到下一相
    m_currentPhaseIndex = (m_currentPhaseIndex + 1) % PHASE_COUNT;

    // 配置下一相的ADC通道
    ADC_ChannelConfTypeDef sConfig = {0};
    sConfig.Channel = m_adcChannels[m_currentPhaseIndex];
    sConfig.Rank = 1;
    sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5;
    HAL_ADC_ConfigChannel(&hadc1, &sConfig);

    // 如果完成了一轮采样，设置完成标志
    if (m_currentPhaseIndex == 0) {
        m_conversionComplete = true;
    }
    // 启动下一次转换
    HAL_ADC_Start_IT(&hadc1);



}

// ADC值转换为电流值
float INA199A1DCKR::convertToCurrent(uint16_t adcValue) const {
    float voltage = (float)adcValue * 3.3f / 4095.0f;

    // 可选：测一次空载时的电压做为基准零点，比如 1.62V
    constexpr float V_zero = 1.65f; // 或者你实际测得的空载输出电压

    float delta_v = voltage - V_zero;

    // 如果是单向INA199A1，且电流反向导致负值，你可能需要处理负值逻辑：
    if (delta_v < 0) delta_v = 0; // 或者报警提示方向错误

    return delta_v / (m_inaGain * m_shuntResistance);
}


// 获取指定相的电流值
float INA199A1DCKR::getCurrentValue(Phase phase) const {
    if (phase < PHASE_COUNT) {
        return m_currentValues[phase];
    }
    return 0.0f;
}

// 获取所有相的电流值数组
std::array<float, PHASE_COUNT> INA199A1DCKR::getAllCurrentValues() const {
    return m_currentValues;
}

// 获取转换完成标志
bool INA199A1DCKR::isConversionComplete() const {
    return m_conversionComplete;
}

// 清除转换完成标志
void INA199A1DCKR::clearConversionFlag() {
    m_conversionComplete = false;
}

#include "../MT6701CT_STD/MT6701CT_STD.h"
#include "../algorithm/filter.h"
#include "arm_math.h"
#include "../FOC/FOCtroler.h"
#include <algorithm> // 引入标准算法库
using namespace std;
extern float encoder_angle;
extern float motor_logic_angle;
extern float sin_value ;
extern float cos_value ;
extern float motor_i_d;
extern float motor_i_q;
float duty_u;
float duty_v;
float duty_w;

float q=0;

CFOCController focController;

float filter_alpha_i_d = 0.1;
float filter_alpha_i_q = 0.1;
// 外部C函数，用于在中断处理函数中调用


void mysetPwm(float duty_u, float duty_v, float duty_w) {
    duty_u = std::min(duty_u, 0.9f); // 留出10%的电流采样时间
    duty_v = min(duty_v, 0.9f);
    duty_w = min(duty_w, 0.9f);
    __disable_irq();
    uint32_t arr = htim1.Instance->ARR;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty_u * arr);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty_v * arr);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty_w * arr);
    __enable_irq();
}

float test_angle = 0.0f;    // 开环测试用的电角度
float angle_step = 0.1f;    // 每次步进的角度（弧度）

extern "C" void ADC1_2_IRQHandler() {
    HAL_ADC_IRQHandler(&hadc1);

    // 1. 处理ADC采样与电流计算
    INA199A1DCKR::instance().adcCallback();
    current_u = INA199A1DCKR::instance().getCurrentValue(PHASE_U);
    current_v = INA199A1DCKR::instance().getCurrentValue(PHASE_V);

    // 2. 更新编码器角度
    encoder_angle=MT6701Sensor::getInstance().getRawAngle();
    //motor_logic_angle = MT6701Sensor::getInstance().getElectricalAngle();

    // 3. 计算三角函数值（使用最新角度）
    sin_value = arm_sin_f32(rotor_logic_angle);
    cos_value = arm_cos_f32(rotor_logic_angle);

    // 4. Clarke变换（相电流 -> αβ坐标系）
    float32_t i_alpha, i_beta;
    arm_clarke_f32(current_u, current_v, &i_alpha, &i_beta);

    // 5. Park变换（αβ坐标系 -> dq坐标系）
    float32_t i_d, i_q;
    arm_park_f32(i_alpha, i_beta, &i_d, &i_q, sin_value, cos_value);

    // 6. 低通滤波（可选）
    motor_i_d = low_pass_filter(i_d, motor_i_d, filter_alpha_i_d);
    motor_i_q = low_pass_filter(i_q, motor_i_q, filter_alpha_i_q);

    // 7. 速度环控制（或直接设置电流指令）
    float target_i_q = 0.0f;  // 增大初始电流值
    float target_i_d = 0.0f;  // 通常保持d轴电流为0

    target_i_q = focController.runSpeedLoop(1);
    // 8. SVPWM调制
    focController.svpwm(rotor_logic_angle, target_i_d, 0.3, &duty_u, &duty_v, &duty_w);

    // 9. 限制PWM占空比
    duty_u = std::min(duty_u, 0.9f);
    duty_v = std::min(duty_v, 0.9f);
    duty_w = std::min(duty_w, 0.9f);

    // 10. 设置PWM输出
    mysetPwm(duty_u, duty_v, duty_w);

    // 直接使用测试角度和固定电流指令
    // focController.svpwm(test_angle, 0.0f, 1.0f, &duty_u, &duty_v, &duty_w);
    // test_angle+=angle_step;
    // // 限制占空比并输出 PWM
    // duty_u = std::min(duty_u, 0.9f);
    // duty_v = std::min(duty_v, 0.9f);
    // duty_w = std::min(duty_w, 0.9f);
    // mysetPwm(duty_u, duty_v, duty_w);
}
